/*
 *  Rb3d4p.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 21/03/11
 *	Modified by Roberto Ortega 01/04/2011
 */

#include "rb3d4p.h"

Rb3d4p::Rb3d4p()
:	Component(),
	mass(0.0),
	rj(),
	rk(),
	rl(),
	A(0.0),
	B(0.0),
	C(0.0)
{}

// ***
Rb3d4p::Rb3d4p(double cmass,
			   vector<string>& mn,
			   vector<vector<double> >& mrel,
			   vector<double>& pmInertia,
			   int di)
:	Component(mn,di),
	mass(cmass)
{
	// First point is assumed to be G
	// Local system assumed to be principal
	
	A = pmInertia[0];
	B = pmInertia[1];	
	C = pmInertia[2];
	
	rj = mrel[0];
	rk = mrel[1];
	rl = mrel[2];
}

//***
void Rb3d4p::calcFi(const vector<double>& qddot)
{
	int i;
	int j;
	
	for(i = 0; i < numDofs; ++i)
	{
		Fi[i] = 0.0;
		
		for(j = 0; j < numDofs; ++j)
		{
			if(integratorType == 0)
				Fi[i] += M(i,j) * qddot[localGlob[j]];
			else
				Fi[i] += M(i,j) * (dq[j] - dqC[j]);
		}
	}
}

// ***
void Rb3d4p::calcFq(const int& nt)
{}

// ***
void Rb3d4p::calcFt(const double& t, const double& grav, const int& gravDir)
{}

// ***
void Rb3d4p::calcKi(double & partial_qddot)
{
	if(partial_qddot != partial_qddotOld)	// In order to compute Ki just once
	{
//		int i;
//		int j;
//		for (i = 0; i < numDofs; i++)
//		{
//			for (j = 0; j < numDofs; j++)
//			{
//				Ki[i][j] = M(i,j)* partial_qddot;
//			}
//		}
		Ki = M * partial_qddot;
	}
	partial_qddotOld = partial_qddot;	
}

// ***
void Rb3d4p::calcKq(int nt)
{}	

// ***
void Rb3d4p::calcKdq(double & partial_qdot)
{}

// ***
void Rb3d4p::calcM(const vector<double>& q, const double& t)
{
	int i;
	
	if (M.size()==0)
	{
		M.setZero(numDofs,numDofs);
	}
	
	double AA = 0.5 * (B + C - A);
	double BB = 0.5 * (A + C - B);
	double CC = 0.5 * (A + B - C);
	
	//cout << AA << "\n";
	//cout << BB << "\n";
	//cout << CC << "\n";
	
	vector<vector<double> > Z;
	
	Z.resize(3);
	for (i = 0; i < 3; i++)
		Z[i].resize(3);
	
	vector<double> ubar(3);
	vector<double> vbar(3);
	vector<double> vij(3), vik(3), vil(3), vjk(3), vjl(3), vkl(3);
	
	for (i=0; i<3; ++i)	vij[i] = q[3+i]-q[i];
	for (i=0; i<3; ++i)	vik[i] = q[6+i]-q[i];
	for (i=0; i<3; ++i)	vil[i] = q[9+i]-q[i];
	for (i=0; i<3; ++i)	vjk[i] = q[6+i]-q[3+i];
	for (i=0; i<3; ++i)	vjl[i] = q[9+i]-q[3+i];
	for (i=0; i<3; ++i)	vkl[i] = q[9+i]-q[6+i];
	
	double lij = lTwoNorm(vij);
	double lik = lTwoNorm(vik);
	double lil = lTwoNorm(vil);
	double ljk = lTwoNorm(vjk);
	double ljl = lTwoNorm(vjl);
	double lkl = lTwoNorm(vkl);
	
	for (i=0; i<3; ++i)
	{
		ubar[i] = rk[i]/lik;
		vbar[i] = (rl[i]-rj[i])/ljl;
	}
	
	Z[0][0] = rj[0]*rj[0]/AA + rj[1]*rj[1]/BB + rj[2]*rj[2]/CC ;
	Z[0][1] = rj[0]*ubar[0]/AA + rj[1]*ubar[1]/BB + rj[2]*ubar[2]/CC ;
	Z[0][2] = rj[0]*vbar[0]/AA + rj[1]*vbar[1]/BB + rj[2]*vbar[2]/CC ;
	Z[1][0] = Z[0][1];
	Z[1][1] = ubar[0]*ubar[0]/AA + ubar[1]*ubar[1]/BB + ubar[2]*ubar[2]/CC ;
	Z[1][2] = ubar[0]*vbar[0]/AA + ubar[1]*vbar[1]/BB + ubar[2]*vbar[2]/CC ;
	Z[2][0] = Z[0][2];
	Z[2][1] = Z[1][2];
	Z[2][2] = vbar[0]*vbar[0]/AA + vbar[1]*vbar[1]/BB + vbar[2]*vbar[2]/CC ;
	
	// Z is overwritten by its inverse
	inverse(Z);	
	
	M.setZero(numDofs,numDofs);
	
	// First row of submatrices 3x3
	M(0,0) = mass + Z[0][0] + (2.0*Z[0][1] + Z[1][1]/lik)/lik; 
	M(1,1) = mass + Z[0][0] + (2.0*Z[0][1] + Z[1][1]/lik)/lik;
	M(2,2) = mass + Z[0][0] + (2.0*Z[0][1] + Z[1][1]/lik)/lik;
	
	M(0,3) = -Z[0][0] - Z[1][0]/lik + (Z[0][2] + Z[1][2]/lik)/ljl; 
	M(1,4) = -Z[0][0] - Z[1][0]/lik + (Z[0][2] + Z[1][2]/lik)/ljl; 
	M(2,5) = -Z[0][0] - Z[1][0]/lik + (Z[0][2] + Z[1][2]/lik)/ljl; 
	
	M(0,6) = -(Z[0][1] + Z[1][1]/lik)/lik;
	M(1,7) = -(Z[0][1] + Z[1][1]/lik)/lik;
	M(2,8) = -(Z[0][1] + Z[1][1]/lik)/lik;
	
	M(0,9) = -(Z[0][2] + Z[1][2]/lik)/ljl; 
	M(1,10) = -(Z[0][2] + Z[1][2]/lik)/ljl; 
	M(2,11) = -(Z[0][2] + Z[1][2]/lik)/ljl; 
	
	// Second row of submatrices 3x3
	M(3,0) = M(0,3);           
	M(4,1) = M(1,4); 
	M(5,2) = M(2,5); 
	
	M(3,3) = Z[0][0] - (2.0*Z[0][2] - Z[2][2]/ljl)/ljl; 
	M(4,4) = Z[0][0] - (2.0*Z[0][2] - Z[2][2]/ljl)/ljl; 
	M(5,5) = Z[0][0] - (2.0*Z[0][2] - Z[2][2]/ljl)/ljl; 
	
	M(3,6) = (Z[0][1] - Z[2][1]/ljl)/lik; 
	M(4,7) = (Z[0][1] - Z[2][1]/ljl)/lik; 
	M(5,8) = (Z[0][1] - Z[2][1]/ljl)/lik; 
	
	M(3,9) =  (Z[0][2] - Z[2][2]/ljl)/ljl; 
	M(4,10) = (Z[0][2] - Z[2][2]/ljl)/ljl; 
	M(5,11) = (Z[0][2] - Z[2][2]/ljl)/ljl;
	
	// Third row of submatrices 3x3 
	M(6,0) = M(0,6);         
	M(7,1) = M(1,7); 
	M(8,2) = M(2,8); 
	
	M(6,3) = M(3,6); 
	M(7,4) = M(4,7); 
	M(8,5) = M(5,8); 
	
	M(6,6) = Z[1][1]/(lik*lik); 
	M(7,7) = Z[1][1]/(lik*lik); 
	M(8,8) = Z[1][1]/(lik*lik); 
	
	M(6,9) =  Z[1][2]/(lik*ljl); 
	M(7,10) = Z[1][2]/(lik*ljl); 
	M(8,11) = Z[1][2]/(lik*ljl); 
	
	// Four row of submatrices 3x3 
	M(9,0) =  M(0,9);
	M(10,1) = M(1,10); 
	M(11,2) = M(2,11); 
	
	M(9,3) =  M(3,9); 
	M(10,4) = M(4,10); 
	M(11,5) = M(5,11); 
	
	M(9,6) =  M(6,9); 
	M(10,7) = M(7,10); 
	M(11,8) = M(8,11); 
	
	M(9,9) =   Z[2][2]/(ljl*ljl); 
	M(10,10) = Z[2][2]/(ljl*ljl); 
	M(11,11) = Z[2][2]/(ljl*ljl);	
}

// ***
void Rb3d4p::calcMLumped(const vector<double>& q, const double& t)
{
	// Diagonal terms of the consistent matrix, scaled to account for the total mass
	int i;
	if(M.size()==0)
	{	
		M.setZero(numDofs,numDofs);
		calcM(q,t);	// Mass matrix can be defined once for all, since it is constant
	}
	if (MLumped.size()==0)
	{
		MLumped.resize(numDofs);
	}
	
	// Scaling
	double sumDiag = 0.0;
	for (i=0; i<M.rows(); ++i)	sumDiag+= M(i,i);
	
	for (i=0; i<numDofs; ++i)	MLumped[i] = M(i,i)*mass*dim/sumDiag;	
	
}

// ***
void Rb3d4p::setDofs(void)
{
	map<string,Point*,less<string> >::iterator i;
	
	/** Loop on points belonging to this component */
	for(i = mPoints.begin(); i != mPoints.end(); i++)	
	{
		for(int j = 0; j < dim; j++)
			(*i).second->setDof(0,j);	// Only translational dof */
		for(int j = 0; j < dim; j++)
			(*i).second->setDofInertia(0,j);
	}
	
	numDofs = dim * pointNames.size();
	numDofsInertia = numDofs;
}

// ***
void Rb3d4p::initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double & t)
{
	int i;
	int j;
	int p;
	
	// Initial position
	q.resize(numDofs);
	
	i = 0;
	for (p = 0; p < pointNames.size(); ++p)
	{
		for (j = 0; j < dim; j++)
		{
			// Default: initial position dofs are point coordinates
			// they go to the firsts positions
			q_in[ localGlob[i * dim + j] ] = mPoints[pointNames[p]]->r[j];
		}
		i++;
	}
	
	// WARNING:
	// Remember that the ordering of vector pointNames and map mPoints is different
	
	// Initial velocity
	// Default: initial velocity dofs are null
	dq.resize(numDofs);
	
}

// ***
void Rb3d4p::setMatAtComp(Material* ma)
{}

// ***
void Rb3d4p::initializeCompRes(const bool& st, const double& grav, const int& gravDir, const bool augl)
{
	if(st != 0)	// Dynamic
	{
		Fi.resize(numDofs);
	}
	if(grav != 0.0)
	{
		// First point is assumed to be G
		
		Ft.resize(numDofs);
		Ft[gravDir - 1] = - mass * grav;
	}
}

// ***
//void Rb3d4p::initializeCompTang(const bool& st,  const bool& pf)
void Rb3d4p::initializeCompTang(const bool& st)
{
	if(st!=0)
	{
//		int i;
//
//		Ki.resize(numDofs);
//		for(i = 0; i < numDofs; ++i)	Ki[i].resize(numDofs);
		Ki.setZero(numDofs,numDofs);
	}	
}
/*
// ***
void Rb3d4p::makeM()
{
	int i;
	
	double AA = 0.5 * (B + C - A);
	double BB = 0.5 * (A + C - B);
	double CC = 0.5 * (A + B - C);
	
	//cout << AA << "\n";
	//cout << BB << "\n";
	//cout << CC << "\n";
	
	vector<vector<double> > Z;
	
	Z.resize(3);
	for (i = 0; i < 3; i++)
		Z[i].resize(3);
	
	vector<double> ubar(3);
	vector<double> vbar(3);
	vector<double> vij(3), vik(3), vil(3), vjk(3), vjl(3), vkl(3);

	for (i=0; i<3; ++i)	vij[i] = q[3+i]-q[i];
	for (i=0; i<3; ++i)	vik[i] = q[6+i]-q[i];
	for (i=0; i<3; ++i)	vil[i] = q[9+i]-q[i];
	for (i=0; i<3; ++i)	vjk[i] = q[6+i]-q[3+i];
	for (i=0; i<3; ++i)	vjl[i] = q[9+i]-q[3+i];
	for (i=0; i<3; ++i)	vkl[i] = q[9+i]-q[6+i];

	double lij = lTwoNorm(vij);
	double lik = lTwoNorm(vik);
	double lil = lTwoNorm(vil);
	double ljk = lTwoNorm(vjk);
	double ljl = lTwoNorm(vjl);
	double lkl = lTwoNorm(vkl);
	
	for (i=0; i<3; ++i)
	{
		ubar[i] = rk[i]/lik;
		vbar[i] = (rl[i]-rj[i])/ljl;
	}
	
	Z[0][0] = rj[0]*rj[0]/AA + rj[1]*rj[1]/BB + rj[2]*rj[2]/CC ;
	Z[0][1] = rj[0]*ubar[0]/AA + rj[1]*ubar[1]/BB + rj[2]*ubar[2]/CC ;
	Z[0][2] = rj[0]*vbar[0]/AA + rj[1]*vbar[1]/BB + rj[2]*vbar[2]/CC ;
	Z[1][0] = Z[0][1];
	Z[1][1] = ubar[0]*ubar[0]/AA + ubar[1]*ubar[1]/BB + ubar[2]*ubar[2]/CC ;
	Z[1][2] = ubar[0]*vbar[0]/AA + ubar[1]*vbar[1]/BB + ubar[2]*vbar[2]/CC ;
	Z[2][0] = Z[0][2];
	Z[2][1] = Z[1][2];
	Z[2][2] = vbar[0]*vbar[0]/AA + vbar[1]*vbar[1]/BB + vbar[2]*vbar[2]/CC ;
	
	// Z is overwritten by its inverse
	inverse(Z);	
	
	setZero(M);
	
	// First row of submatrices 3x3
	M[0][0] = mass + Z[0][0] + (2.0*Z[0][1] + Z[1][1]/lik)/lik; 
	M[1][1] = mass + Z[0][0] + (2.0*Z[0][1] + Z[1][1]/lik)/lik;
	M[2][2] = mass + Z[0][0] + (2.0*Z[0][1] + Z[1][1]/lik)/lik;
	
	M[0][3] = -Z[0][0] - Z[1][0]/lik + (Z[0][2] + Z[1][2]/lik)/ljl; 
	M[1][4] = -Z[0][0] - Z[1][0]/lik + (Z[0][2] + Z[1][2]/lik)/ljl; 
	M[2][5] = -Z[0][0] - Z[1][0]/lik + (Z[0][2] + Z[1][2]/lik)/ljl; 
	
	M[0][6] = -(Z[0][1] + Z[1][1]/lik)/lik;
	M[1][7] = -(Z[0][1] + Z[1][1]/lik)/lik;
	M[2][8] = -(Z[0][1] + Z[1][1]/lik)/lik;
	
	M[0][9] = -(Z[0][2] + Z[1][2]/lik)/ljl; 
	M[1][10] = -(Z[0][2] + Z[1][2]/lik)/ljl; 
	M[2][11] = -(Z[0][2] + Z[1][2]/lik)/ljl; 
	
	// Second row of submatrices 3x3
	M[3][0] = M[0][3];           
	M[4][1] = M[1][4]; 
	M[5][2] = M[2][5]; 
	
	M[3][3] = Z[0][0] - (2.0*Z[0][2] - Z[2][2]/ljl)/ljl; 
	M[4][4] = Z[0][0] - (2.0*Z[0][2] - Z[2][2]/ljl)/ljl; 
	M[5][5] = Z[0][0] - (2.0*Z[0][2] - Z[2][2]/ljl)/ljl; 
	
	M[3][6] = (Z[0][1] - Z[2][1]/ljl)/lik; 
	M[4][7] = (Z[0][1] - Z[2][1]/ljl)/lik; 
	M[5][8] = (Z[0][1] - Z[2][1]/ljl)/lik; 
	
	M[3][9] =  (Z[0][2] - Z[2][2]/ljl)/ljl; 
	M[4][10] = (Z[0][2] - Z[2][2]/ljl)/ljl; 
	M[5][11] = (Z[0][2] - Z[2][2]/ljl)/ljl;
	
	// Third row of submatrices 3x3 
	M[6][0] = M[0][6];         
	M[7][1] = M[1][7]; 
	M[8][2] = M[2][8]; 
	
	M[6][3] = M[3][6]; 
	M[7][4] = M[4][7]; 
	M[8][5] = M[5][8]; 
	
	M[6][6] = Z[1][1]/(lik*lik); 
	M[7][7] = Z[1][1]/(lik*lik); 
	M[8][8] = Z[1][1]/(lik*lik); 
	
	M[6][9] =  Z[1][2]/(lik*ljl); 
	M[7][10] = Z[1][2]/(lik*ljl); 
	M[8][11] = Z[1][2]/(lik*ljl); 
	
	// Four row of submatrices 3x3 
	M[9][0] =  M[0][9];
	M[10][1] = M[1][10]; 
	M[11][2] = M[2][11]; 
	
	M[9][3] =  M[3][9]; 
	M[10][4] = M[4][10]; 
	M[11][5] = M[5][11]; 
	
	M[9][6] =  M[6][9]; 
	M[10][7] = M[7][10]; 
	M[11][8] = M[8][11]; 
	
	M[9][9] =   Z[2][2]/(ljl*ljl); 
	M[10][10] = Z[2][2]/(ljl*ljl); 
	M[11][11] = Z[2][2]/(ljl*ljl);
}
*/

// ***
void Rb3d4p::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint,double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in)
{
	int i, j;
	
	double V;
	double T;
	
	/* Kinentic energy */ 
	if(dq.size()!=0)
	{
		for (i=0; i<numDofs; ++i)
			dq[i] = dq_in[localGlob[i]];

		T = 0.0;
		for (i=0; i<numDofs; ++i)
		{			
			for (j=0; j<numDofs; ++j)
			{
				T += 0.5*dq[i]*M(i,j)*dq[j];
			}
		}
	}
	Ekin += T;
	
	/* Potential energy */ 
	V = 0.0;
	if (grav!=0.0)
	{
		V += grav*mass*q[gravDir-1];
	}
	Epot += V;
	//cout << "T: " << T << "\t" << "V: " << V << endl;
}

// ***
void Rb3d4p::calcEntropy(double& Entropy, double& EntropyAmb)
{}

// ***
void Rb3d4p::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{}	

// ***
void Rb3d4p::updateComp(void)
{}

// ***
void Rb3d4p::setIniDofs(vector<double>&)
{}

// ***
void Rb3d4p::calcPhi(const vector<double>& Q)
{}

// ***
void Rb3d4p::calcDPhi(const vector<double>& Q)
{}

// ***

void Rb3d4p::calcMSmooth(const vector<double>& q_in, const double& t)
{
//	int i;
//	if(MSmooth.size()==0)
//	{
//		MSmooth.resize(numDofsInertia);
//		for( i=0;i<numDofsInertia;i++) MSmooth[i].resize(numDofsInertia);
//	}
	MSmooth.setZero(numDofsInertia,numDofsInertia);

	MSmooth = MatrixXd::Identity(numDofsInertia,numDofsInertia);
//	for(i=0; i<numDofsInertia;i++) MSmooth[i][i] = 1.0;
}

// ***

void Rb3d4p::calcFSmooth(const vector<double>& q_in, const vector<double>& dq_in, const double& t)
{
	FSmooth.setZero(2,numDofsInertia);
//	int i;
//	if (FSmooth.size()==0)		// Just in case M has not been created in initializeCompRes
//	{
//		FSmooth.resize(2);
//		for (i=0; i<2; ++i)	FSmooth[i].resize(numDofsInertia);
//	}
//	else setZero(FSmooth);
}

int Rb3d4p::checkState()
{
return 0;
}
